#include "opengldisplay.h"

OpenGLDisplay::OpenGLDisplay(QWidget *parent) :
    QGLWidget(parent),
    _controller(NULL),
    _model(NULL)
{
    setMouseTracking(true);
}

OpenGLDisplay::~OpenGLDisplay(){
}

void OpenGLDisplay::loadController(RobotGLController *controller){
    if(_controller != NULL)
        delete _controller;

    _controller = controller;
    //Establish a link from the controller to this view
    _controller->loadView(this);
}

void OpenGLDisplay::loadModel(Robot *robot){
    if(_model != NULL)
        delete _model;

    _model = robot;
    //Listen to the model for changes that need to update the view
    connect(_model, SIGNAL(robotChanged()),this, SLOT(updateGL()));

    //Pass the model to the controller
    _controller->loadModel(robot);

    //Initialize the gui components
    initComponents();
}

/********************************************************/
/*              UTILITY FUNCTIONS                       */
/********************************************************/
QPointF OpenGLDisplay::transformCoordinates(QPointF point){
    QPointF ret(point.x(), ((this->height() - 1) - point.y()));
    return ret;
}

QPoint OpenGLDisplay::transformCoordinates(QPoint point){
    QPoint ret(point.x(), ((this->height() - 1) - point.y()));
    return ret;
}

int OpenGLDisplay::transformYCoordinate(int y){
    return ((this->height() - 1) - y);
}

void OpenGLDisplay::transformTo3D(float winX, float winY, double *posX, double *posY, double *posZ){
    int viewport[4];
    double modelview[16];
    double projection[16];
    float winZ;

    glGetDoublev(GL_MODELVIEW_MATRIX, modelview);
    glGetDoublev(GL_PROJECTION_MATRIX, projection);
    glGetIntegerv(GL_VIEWPORT, viewport);

    glReadPixels(winX, transformYCoordinate((int)winY), 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &winZ);
    //TODO: Try and figure out the magic 0.95 number
    gluUnProject(winX, transformYCoordinate((int)winY), 0.95, modelview, projection, viewport, posX, posY, posZ);
}


void OpenGLDisplay::wireCube (float size){
  glPushMatrix();

  //Draws a polygon for hits, but uses GL_LINE mode
  //to render it as a wireframe
  glPushAttrib(GL_POLYGON_MODE);
  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
  glBegin(GL_POLYGON);

    //back face
    glVertex3f(-0.5 * size, -0.5 * size, -0.5 * size);
    //Setting the edge flag false in the appropriate spots
    //eliminates criss-crossing lines
    glEdgeFlag(GL_FALSE);
    glVertex3f(-0.5 * size, +0.5 * size, -0.5 * size);
    glEdgeFlag(GL_TRUE);

    glVertex3f(+0.5 * size, -0.5 * size, -0.5 * size);
    glEdgeFlag(GL_FALSE);
    glVertex3f(+0.5 * size, +0.5 * size, -0.5 * size);
    glEdgeFlag(GL_TRUE);

    glVertex3f(-0.5 * size, -0.5 * size, -0.5 * size);
    glVertex3f(+0.5 * size, -0.5 * size, -0.5 * size);

    glVertex3f(+0.5 * size, +0.5 * size, -0.5 * size);
    glEdgeFlag(GL_FALSE);
    glVertex3f(-0.5 * size, +0.5 * size, -0.5 * size);
    glEdgeFlag(GL_TRUE);

    //front face
    glVertex3f(-0.5 * size, -0.5 * size, +0.5 * size);
    glEdgeFlag(GL_FALSE);
    glVertex3f(-0.5 * size, +0.5 * size, +0.5 * size);
    glEdgeFlag(GL_TRUE);

    glVertex3f(+0.5 * size, -0.5 * size, +0.5 * size);
    glEdgeFlag(GL_FALSE);
    glVertex3f(+0.5 * size, +0.5 * size, +0.5 * size);
    glEdgeFlag(GL_TRUE);

    glVertex3f(-0.5 * size, -0.5 * size, +0.5 * size);
    glVertex3f(+0.5 * size, -0.5 * size, +0.5 * size);

    glVertex3f(+0.5 * size, +0.5 * size, +0.5 * size);
    glEdgeFlag(GL_FALSE);
    glVertex3f(-0.5 * size, +0.5 * size, +0.5 * size);
    glEdgeFlag(GL_TRUE);

    //join back to front
    glVertex3f(-0.5 * size, -0.5 * size, -0.5 * size);
    glEdgeFlag(GL_FALSE);
    glVertex3f(-0.5 * size, -0.5 * size, +0.5 * size);
    glEdgeFlag(GL_TRUE);

    glVertex3f(-0.5 * size, +0.5 * size, -0.5 * size);
    glEdgeFlag(GL_FALSE);
    glVertex3f(-0.5 * size, +0.5 * size, +0.5 * size);
    glEdgeFlag(GL_TRUE);

    glVertex3f(+0.5 * size, -0.5 * size, -0.5 * size);
    glEdgeFlag(GL_FALSE);
    glVertex3f(+0.5 * size, -0.5 * size, +0.5 * size);
    glEdgeFlag(GL_TRUE);

    glVertex3f(+0.5 * size, +0.5 * size, -0.5 * size);
    glEdgeFlag(GL_FALSE);
    glVertex3f(+0.5 * size, +0.5 * size, +0.5 * size);
    glEdgeFlag(GL_TRUE);

  glEnd();
  glPopMatrix();
  glPopAttrib();
}

void OpenGLDisplay::shadedCube (float size){
  glPushMatrix();
  glPushAttrib(GL_POLYGON_MODE);

  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
  glBegin(GL_QUADS);

    //back face
    glNormal3f(0,0,-1);
    glVertex3f(+0.5 * size, -0.5 * size, -0.5 * size);
    glNormal3f(0,0,-1);
    glVertex3f(+0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(0,0,-1);
    glVertex3f(-0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(0,0,-1);
    glVertex3f(-0.5 * size, -0.5 * size, -0.5 * size);

    //front face
    glNormal3f(0,0,1);
    glVertex3f(+0.5 * size, -0.5 * size, +0.5 * size);
    glNormal3f(0,0,1);
    glVertex3f(+0.5 * size, +0.5 * size, +0.5 * size);
    glNormal3f(0,0,1);
    glVertex3f(-0.5 * size, +0.5 * size, +0.5 * size);
    glNormal3f(0,0,1);
    glVertex3f(-0.5 * size, -0.5 * size, +0.5 * size);

    //top face
    glNormal3f(0,1,0);
    glVertex3f(-0.5 * size, +0.5 * size, +0.5 * size);
    glNormal3f(0,1,0);
    glVertex3f(-0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(0,1,0);
    glVertex3f(+0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(0,1,0);
    glVertex3f(+0.5 * size, +0.5 * size, +0.5 * size);

    //bottom face
    glNormal3f(0,-1,0);
    glVertex3f(+0.5 * size, -0.5 * size, +0.5 * size);
    glNormal3f(0,-1,0);
    glVertex3f(+0.5 * size, -0.5 * size, -0.5 * size);
    glNormal3f(0,-1,0);
    glVertex3f(-0.5 * size, -0.5 * size, -0.5 * size);
    glNormal3f(0,-1,0);
    glVertex3f(-0.5 * size, -0.5 * size, +0.5 * size);


    //left face
    glNormal3f(-1,0,0);
    glVertex3f(-0.5 * size, +0.5 * size, +0.5 * size);
    glNormal3f(-1,0,0);
    glVertex3f(-0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(-1,0,0);
    glVertex3f(-0.5 * size, -0.5 * size, -0.5 * size);
    glNormal3f(-1,0,0);
    glVertex3f(-0.5 * size, -0.5 * size, +0.5 * size);

    //right face
    glNormal3f(1,0,0);
    glVertex3f(+0.5 * size, +0.5 * size, +0.5 * size);
    glNormal3f(1,0,0);
    glVertex3f(+0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(1,0,0);
    glVertex3f(+0.5 * size, -0.5 * size, -0.5 * size);
    glNormal3f(1,0,0);
    glVertex3f(+0.5 * size, -0.5 * size, +0.5 * size);

  glEnd();

  glDisable(GL_LIGHTING);
  glColor3f(0.25f, 0.27f, 0.42f);
  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

  glLineWidth(3);
  glBegin(GL_QUADS);

    //back face
    glNormal3f(0,0,-1);
    glVertex3f(+0.5 * size, -0.5 * size, -0.5 * size);
    glNormal3f(0,0,-1);
    glVertex3f(+0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(0,0,-1);
    glVertex3f(-0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(0,0,-1);
    glVertex3f(-0.5 * size, -0.5 * size, -0.5 * size);

    //front face
    glNormal3f(0,0,1);
    glVertex3f(+0.5 * size, -0.5 * size, +0.5 * size);
    glNormal3f(0,0,1);
    glVertex3f(+0.5 * size, +0.5 * size, +0.5 * size);
    glNormal3f(0,0,1);
    glVertex3f(-0.5 * size, +0.5 * size, +0.5 * size);
    glNormal3f(0,0,1);
    glVertex3f(-0.5 * size, -0.5 * size, +0.5 * size);

    //top face
    glNormal3f(0,1,0);
    glVertex3f(-0.5 * size, +0.5 * size, +0.5 * size);
    glNormal3f(0,1,0);
    glVertex3f(-0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(0,1,0);
    glVertex3f(+0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(0,1,0);
    glVertex3f(+0.5 * size, +0.5 * size, +0.5 * size);

    //bottom face
    glNormal3f(0,-1,0);
    glVertex3f(+0.5 * size, -0.5 * size, +0.5 * size);
    glNormal3f(0,-1,0);
    glVertex3f(+0.5 * size, -0.5 * size, -0.5 * size);
    glNormal3f(0,-1,0);
    glVertex3f(-0.5 * size, -0.5 * size, -0.5 * size);
    glNormal3f(0,-1,0);
    glVertex3f(-0.5 * size, -0.5 * size, +0.5 * size);


    //left face
    glNormal3f(-1,0,0);
    glVertex3f(-0.5 * size, +0.5 * size, +0.5 * size);
    glNormal3f(-1,0,0);
    glVertex3f(-0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(-1,0,0);
    glVertex3f(-0.5 * size, -0.5 * size, -0.5 * size);
    glNormal3f(-1,0,0);
    glVertex3f(-0.5 * size, -0.5 * size, +0.5 * size);

    //right face
    glNormal3f(1,0,0);
    glVertex3f(+0.5 * size, +0.5 * size, +0.5 * size);
    glNormal3f(1,0,0);
    glVertex3f(+0.5 * size, +0.5 * size, -0.5 * size);
    glNormal3f(1,0,0);
    glVertex3f(+0.5 * size, -0.5 * size, -0.5 * size);
    glNormal3f(1,0,0);
    glVertex3f(+0.5 * size, -0.5 * size, +0.5 * size);

  glEnd();

  glLineWidth(1);
  glPopAttrib();
  glPopMatrix();
}


void OpenGLDisplay::wireSphere(float radius, int slices, int stacks){
    GLUquadricObj* quadric = gluNewQuadric();
    gluQuadricDrawStyle(quadric, GLU_LINE);
    gluSphere(quadric, radius, slices, stacks);
    gluDeleteQuadric(quadric);
}


/********************************************************/
/*                  OPENGL FUNCTIONS                    */
/********************************************************/
void OpenGLDisplay::initializeGL(){
    glClearColor(0.77f, 0.77f, 0.75f, 0.0f);

    glEnable(GL_LIGHT0);
    GLfloat lightpos[] = {0.0f, 1.f, 0.5f, 0.};
    glLightfv(GL_LIGHT0, GL_POSITION, lightpos);

    GLfloat amb[] = {0.35f, 0.35f, 0.35f, 0.};
    glLightfv(GL_LIGHT0, GL_AMBIENT, amb);

    GLfloat spec[] = {0.65f, 0.65f, 0.60f, 0.};
    glLightfv(GL_LIGHT0, GL_SPECULAR, spec);

}

void OpenGLDisplay::paintGL(){
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glColor3f(1.0, 1.0, 1.0);

    glPushMatrix();
        glTranslatef(0.0, 0.0, 0.0);
        drawScene();
    glPopMatrix();
}

void OpenGLDisplay::startPicking(int cursorX, int cursorY){
    unsigned int selectBuf[SELECT_BUF_SIZE];
    int hits;
    int viewport[4];

    //Get the viewport for gluPickMatrix
    glGetIntegerv(GL_VIEWPORT, viewport);

    //Switch to the selection buffer
    glSelectBuffer(SELECT_BUF_SIZE, selectBuf);
    glRenderMode(GL_SELECT);

    //Set up the viewport to be a 5x5 area around the mouse cursor,
    //but with the same perspective as we have while rendering
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    gluPickMatrix(cursorX, transformYCoordinate(cursorY), 5, 5, viewport);
    //Need a call to the subclassed function to set the appropriate perspective
    setPerspective(this->width(), this->height());

    //Draw to the selection buffer
    glMatrixMode(GL_MODELVIEW);
    drawScene();

    //Restore the previous projection, etc.
    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);
    glFlush();

    //Switch back to render mode and record the hits
    hits = glRenderMode(GL_RENDER);

    //Process the hits
    processHits(hits, selectBuf);
}

void OpenGLDisplay::processHits(int hits, unsigned int buffer[]){
    unsigned int i, j, names, *ptr, minZ, *ptrNames, numOfNames = 0;
    NameGroup pickedNames;
    QString recordInfo = "";
    QTextStream recordStream(&recordInfo);

    emit processingHits(hits);

    recordStream << "Hits: " << hits << "\n\n";

    //Read the hit records and select the appropriate object
    minZ = 0xffffffff;
    ptr = (unsigned int*)buffer;
    for(i = 0; i < hits; i++){
        names = *ptr;
        recordStream << "Number of names for this hit: " << names << "\n"; ptr++;

        //Find and store info about the closest object
        if(*ptr < minZ){
            numOfNames = names;
            minZ = *ptr;
            ptrNames = ptr+2;
        }
        recordStream << "Min depth: " << (float)*ptr/0x7fffffff << "\n"; ptr++;
        recordStream << "Max depth: " << (float)*ptr/0x7fffffff << "\n"; ptr++;

        recordStream << "Names are:\n";
        for(j = 0; j < names; j++){
            recordStream << "  " << *ptr << "\n";
            ptr++;
        }
    }

    ptr = ptrNames;

    if(numOfNames > 0){
        //Copy the picked names into a NameGroup array
        for(i = 0; i < numOfNames; i++){
            pickedNames[i] = ptr[i];
        }
        for(i = numOfNames; i < MAX_NAMES; i++){
            pickedNames[i] = 0;
        }

        //Tell the controller that a component was picked
        if(_controller != NULL)
            _controller->componentPicked(pickedNames);
    }
    else{
        //Tell the controller that nothing was picked
        _controller->noPick();
    }

    recordStream << "\nClosest name(s):\n";
    for(i = 0; i < numOfNames; i++){
        recordStream << "  " << *ptr << "\n";
        ptr++;
    }
    emit hitInfo(recordInfo);
}


/********************************************************/
/*                 GUICOMPONENT CLASS                   */
/********************************************************/
OpenGLDisplay::GUIComponent::GUIComponent(NameGroup names, bool selectable) :
    _selectable(selectable),
    _selected(false)
{
    int i;
    for(i = 0; i < MAX_NAMES; i++){
        _names[i] = names[i];
    }

    _selectionColor[0] = 0;
    _selectionColor[1] = 0;
    _selectionColor[2] = 1.0;
}

OpenGLDisplay::GUIComponent::GUIComponent(NameGroup names, RGBColor selectionColor,
                                          bool selectable) :
    _selectable(selectable),
    _selected(false)
{
    int i;
    for(i = 0; i < MAX_NAMES; i++){
        _names[i] = names[i];
    }

    for(i = 0; i < 3; i++){
        _selectionColor[i] = selectionColor[i];
    }
}

const unsigned int* OpenGLDisplay::GUIComponent::names() const{
    return _names;
}

const float* OpenGLDisplay::GUIComponent::selectionColor() const{
    return _selectionColor;
}

bool OpenGLDisplay::GUIComponent::selectable() const{
    return _selectable;
}

bool OpenGLDisplay::GUIComponent::selected() const{
    return _selected;
}

void OpenGLDisplay::GUIComponent::setSelectable(bool selectable){
    _selectable = selectable;
}

void OpenGLDisplay::GUIComponent::setSelected(bool selected){
    _selected = selected;
}


OpenGLDisplay::GUIComponent* OpenGLDisplay::findComponent(NameGroup names){
    int i,j;
    bool pass;
    const unsigned int* curNames;

    //Is there a better algorithm than this O(n^2) one?
    for(i = 0; i < _components.length(); i++){
        curNames = _components[i]->names();
        pass = true;

        //Cycle through each name for the current component
        for(j = 0; j < MAX_NAMES; j++){
            if(curNames[j] != names[j]){
                //One of the names didn't match, break
                pass = false;
                break;
            }
        }

        //If pass == true, we matched all names.  Otherwise,
        //continue looping through the components.
        if(pass == true)
            return _components[i];
    }

    return NULL;
}

OpenGLDisplay::GUIComponent* OpenGLDisplay::selectComponent(NameGroup names){
    GUIComponent* component = findComponent(names);
    if(component != NULL && component->selectable()){
        component->setSelected(true);
        emit componentSelected(component->names());
        return component;
    }

    return NULL;
}

void OpenGLDisplay::deselectComponent(NameGroup names){
    GUIComponent* component = findComponent(names);
    if(component != NULL){
        component->setSelected(false);
    }
}

bool OpenGLDisplay::isSelected(NameGroup names){
    GUIComponent* component = findComponent(names);
    if(component != NULL){
        return component->selected();
    }

    return false;
}


void OpenGLDisplay::clearSelection(){
    int i;

    for(i = 0; i < _components.length(); i++){
        _components[i]->setSelected(false);
    }

    emit selectionCleared();
}


/********************************************************/
/*                      EVENTS                          */
/********************************************************/
void OpenGLDisplay::mouseMoveEvent(QMouseEvent *event){
    //Pass the event to the controller
    _controller->mouseMoveEvent(event);

    //emit the gl mouse move event w/ info on the move
    //in 3D space
    double posX, posY, posZ;
    transformTo3D((float)event->x(), (float)event->y(),
                         &posX, &posY, &posZ);

    emit glMouseMove(posX, posY, posZ);
}

void OpenGLDisplay::mousePressEvent(QMouseEvent *event){
    //Pass the event to the controller
    _controller->mousePressEvent(event);

    //Do the openGL picking stuff (which passes its info
    //to the controller)
    startPicking(event->x(), event->y());

    double posX, posY, posZ;
    transformTo3D((float)event->x(), (float)event->y(),
                         &posX, &posY, &posZ);

    emit glMousePress(posX, posY, posZ);
}

void OpenGLDisplay::mouseReleaseEvent(QMouseEvent *event){
    //Pass the event to the controller
    _controller->mouseReleaseEvent(event);

    double posX, posY, posZ;
    transformTo3D((float)event->x(), (float)event->y(),
                         &posX, &posY, &posZ);

    emit glMouseRelease(posX, posY, posZ);
}

/* Create a matrix that will project the desired shadow onto an arbitrary plane.
    Source: OpenGL examples */
void OpenGLDisplay::shadowMatrix(GLfloat shadowMat[4][4], GLfloat groundplane[4], GLfloat lightpos[4])
{
  GLfloat dot;

  /* Find dot product between light position vector and ground plane normal. */
  dot = groundplane[0] * lightpos[0] +
    groundplane[1] * lightpos[1] +
    groundplane[2] * lightpos[2] +
    groundplane[3] * lightpos[3];

  shadowMat[0][0] = dot - lightpos[0] * groundplane[0];
  shadowMat[1][0] = 0.f - lightpos[0] * groundplane[1];
  shadowMat[2][0] = 0.f - lightpos[0] * groundplane[2];
  shadowMat[3][0] = 0.f - lightpos[0] * groundplane[3];

  shadowMat[0][1] = 0.f - lightpos[1] * groundplane[0];
  shadowMat[1][1] = dot - lightpos[1] * groundplane[1];
  shadowMat[2][1] = 0.f - lightpos[1] * groundplane[2];
  shadowMat[3][1] = 0.f - lightpos[1] * groundplane[3];

  shadowMat[0][2] = 0.f - lightpos[2] * groundplane[0];
  shadowMat[1][2] = 0.f - lightpos[2] * groundplane[1];
  shadowMat[2][2] = dot - lightpos[2] * groundplane[2];
  shadowMat[3][2] = 0.f - lightpos[2] * groundplane[3];

  shadowMat[0][3] = 0.f - lightpos[3] * groundplane[0];
  shadowMat[1][3] = 0.f - lightpos[3] * groundplane[1];
  shadowMat[2][3] = 0.f - lightpos[3] * groundplane[2];
  shadowMat[3][3] = dot - lightpos[3] * groundplane[3];

}
